;-------------------------------------------------------------------------------------
;
;	
;							MiTAC Corporation		
;				NO. 187, TIDING BLVD., SEC. 2, TAIPEI, TAIWAN
;	
; 			(c) Copyright 2006, MiTAC Corporation, Taipei, Taiwan  
;						All rights reserved.
;-------------------------------------------------------------------------------------
;
;
;	File:  intrvec.s
;
;	Programer: Visual.Wei 
;	Date: 2006-3-20

        INCLUDE kxarm.h

;----------------------------------------------
INTR_VEC_PHY_BASE			EQU     0x00000000
INTR_STACK_PHY_BASE			EQU     0x30084000
;----------------------------------------------
        TEXTAREA

;----------------vectors------offset: 0x00----------------
_reset	B	boot 			;  RESET INTERRUPT
undef	B	undef			;  UNDEFINED INSTRUCTION INTERRUPT
		B	INT_Swi			;  SOFTWARE INTERRUPT
abort1	B	_EX_Prefetch		;  ABORT (PREFETCH) INTERRUPT
		B	_EX_AbortD		;  ABORT (DATA) INTERRUPT
reserv	B	reserv			;  RESERVED
		B	_EX_Irq			;  IRQ INTERRUPT
		B	_EX_Fiq			 ;  FIQ INTERRUPT
;----------magic bumber---offset: 0x20-----------------
magic           DCD     0x55AA
;-----------exception handler fuction table----------------
func_tb		DCD		0		;1	0x24	reset
			DCD		0		;2	0x28	undefined ins.
			DCD		0		;3	0x2c		swi
			DCD		0		;4	0x30	prefetch
			DCD		0		;5	0x34	data
			DCD		0		;6	0x38	reserve
			DCD		0		;7	0x3c		IRQ
			DCD		0		;8	0x40	FIQ
;-----------------struct regs------defined in kernel_asm.h-----
struct_regs		DCD		0		;FSR		0x44
				DCD		0		;FAR		0x48
				DCD		0		;PC		0x4c
;-----exception handler function table entry address-------
swi_h			DCD		INTR_VEC_PHY_BASE+0x2c	
prefetch_h		DCD		INTR_VEC_PHY_BASE+0x30	
data_h			DCD		INTR_VEC_PHY_BASE+0x34	
irq_h			DCD		INTR_VEC_PHY_BASE+0x3c	
fiq_h			DCD		INTR_VEC_PHY_BASE+0x40	
struct_regs_h 		DCD		INTR_VEC_PHY_BASE+0x44	
stack_base_h 		DCD		INTR_STACK_PHY_BASE	
;----------------exception handler--------------------
;-----------------reset ISR----------------
boot
;-----------------swi ISR----------------
INT_Swi
		stmfd sp!,{r0-r3,r12,lr}	;backup 
		
		ldr r1,(swi_h)
		mov lr,pc
		ldr pc,[r1]


		ldmfd sp!,{r0-r3,r12,pc}^	;restore
;-----------instruction prefetch ISR----------------------
_EX_Prefetch
		subs r14,r14,#4
		;;ldr sp,(stack_base_h)		;set stack
		stmfd sp!,{r0-r3,r12,lr}	;backup 
		
		ldr r1,(prefetch_h)
		mov lr,pc
		ldr pc,[r1]


		ldmfd sp!,{r0-r3,r12,pc}^	;restore
;-----------------data access ISR----------------
_EX_AbortD 
		subs r14,r14,#8
		;;ldr sp,(stack_base_h)		;set stack
		stmfd sp!,{r0-r3,r12,lr}	;backup 

		ldr r0,(struct_regs_h)		;pass parameter
		mrc p15,0,r1,c5,c0,0	;Read c5 to r1: fsr
		str r1,[r0]
		mrc p15,0,r1,c6,c0,0	;Read c6 to r1: far
		str r1,[r0,#4]
		str r14,[r0,#8]			;store instruction error pc
		ldr r1,(data_h)
		mov lr,pc
		ldr pc,[r1]

		ldmfd sp!,{r0-r3,r12,pc}^	;restore
;-----------------swi ISR----------------
_EX_Irq 
		subs r14,r14,#4
		;;ldr sp,(stack_base_h)		;set stack
		stmfd sp!,{r0-r3,r12,lr}	;backup 
		
		ldr r1,(irq_h)
		mov lr,pc
		ldr pc,[r1]

		ldmfd sp!,{r0-r3,r12,pc}^	;restore
;-----------------swi ISR----------------
_EX_Fiq
		subs r14,r14,#4
		stmfd sp!,{r0-r3,r12,lr}	;backup 
		
		ldr r1,(irq_h)
		mov lr,pc
		ldr pc,[r1]


		ldmfd sp!,{r0-r3,r12,pc}^	;restore
_stub_end
;----------------code end-----------------------------------
USERMODE    EQU 	0x10
FIQMODE     EQU 	0x11
IRQMODE     EQU 	0x12
SVCMODE     EQU 	0x13
ABORTMODE   EQU 	0x17
UNDEFMODE   EQU 	0x1b
MODEMASK    EQU 	0x1f
NOINT       EQU 	0xc0
;----------------_trap_init()----------------------------------
		LEAF_ENTRY _trap_init
		ldr	r2, =INTR_VEC_PHY_BASE		;interrupt vector base address
		ldr	r0, =_reset		
		ldr	r1, =_stub_end
1		ldr	r3, [r0], #4
		str	r3, [r2], #4
		cmp	r0, r1
		blt	%B1
;...........................................................
; set each privileged mode stack
	mrs	r0,cpsr
	bic	r0,r0,#MODEMASK
	orr	r1,r0,#UNDEFMODE|NOINT
	msr	cpsr_cxsf,r1		;UndefMode
	ldr	sp,(stack_base_h)

	orr	r1,r0,#ABORTMODE|NOINT
	msr	cpsr_cxsf,r1		;AbortMode
	ldr	sp,(stack_base_h)

	orr	r1,r0,#IRQMODE|NOINT
	msr	cpsr_cxsf,r1		;IRQMode
	ldr	sp,(stack_base_h)

	orr	r1,r0,#FIQMODE|NOINT
	msr	cpsr_cxsf,r1		;FIQMode
	ldr	sp,(stack_base_h)

	bic	r0,r0,#MODEMASK|NOINT
	orr	r1,r0,#SVCMODE
	msr	cpsr_cxsf,r1		;SVCMode
;...........................................................
		;;disable IRQ,FIQ
		mrs     r0, cpsr                        ; Get current mode bits.
		orr     r0, r0, #0xc0                   ;disable IRQs/FIQs
		msr     cpsr_c, r0 
		
		;;low vector
		mrc p15,0,r1,c1,c0,0	;Read c1
		bic r1,r1,#(1:SHL:13)	;clear V bit
		mcr p15,0,r1,c1,c0,0	;write c1,enablehigh vector

		bx	lr
;----------------_enable_irq()----------------------------------
		LEAF_ENTRY _enable_irq
		;;enable IRQ,FIQ
		mrs     r0, cpsr                        ; Get current mode bits.
		bic     r0, r0, #0xc0                   ; enable IRQs/FIQs
		msr     cpsr_c, r0 
		
		bx lr
;----------------_disable_irq()----------------------------------
		LEAF_ENTRY _disable_irq
		;;disable IRQ,FIQ
		mrs     r0, cpsr                        ; Get current mode bits.
		orr     r0, r0, #0xc0                   ; disable IRQs/FIQs
		msr     cpsr_c, r0 
		
		bx lr
;---------------_swi()------------------------
		LEAF_ENTRY _swi
		mov r0,lr
		DCD 0xEF000000		;swi
		bx r0
        END
